/**
 * @file   test001.c
 * @author Scott Craig and Justin Tanner
 * @date   Mon Oct 29 16:19:32 2007
 *
 * @brief  Test 001 - sanity test, can we print to UART
 *
 */

#include "common.h"
#include "OS/os.h"
#include "radio/radio.h"
#include "uart/uart.h"
#include "led/led.h"
#include "app.h"

enum { A=1, B, C, D, E, F, G };
const unsigned int PT = 1;
const unsigned char PPP[] = {A, 20};

static uint8_t radio_buf[PAYLOAD_BYTES];
volatile uint8_t radio_cmd;
volatile STATES state=LOCK_DISABLED;
volatile INPUTS  input=0;
volatile INPUTS  prev_input=0;
char next_lock, prev_lock;
volatile EVENT *input_event;


void app_init(){

    /** Disable default prescaler to make processor speed 8 MHz. */
    //CLKPR = (1<<CLKPCE);
    //CLKPR = 0x00;	


	//BEGIN Servo Motor
	DDRC |= (1<<PORTC5); //Enable output for PSWM

	TCCR3B = (0<<CS30) | (1<<CS31) | (0<<CS32) | (1<<WGM33) | (1<<WGM32); 
	TCCR3A = (0<<WGM30) | (1<<WGM31) | (1<<COM3B1) | (0<<COM3B0);

	ICR3 = 40000; // The max width for the PWM (TOP)
	OCR3B = duty_closed;
	//END Servo Motor

	//BEGIN IR-SENSOR
	PORTE &= ~(PORTE1 | PORTE3);
	//END IR-SENSOR

	//BEGIN LED
	init_LED_D2();
	init_LED_D5();
	//END LED


	//BEGIN RADIO
	radio_init((uint8_t) 120, 0xABBA, 1);
	
	if (LOCK_NUM=='1'){
		uart_init();
		uart_write((uint8_t*)"Connected\r\n", 11);
	}	
	
	//END RADIO
	sei(); /** enable global interrupts **/	
	radio_print("Gate %c: Connected\r\n", LOCK_NUM);	
}

uint8_t inputCount=0;

void task_sampler(){	
	for(;;){
		prev_input=input;
		input=0; //Clear Input

		switch(state){
			case LOCK_CLOSED:
				on_LED(LED_D2_RED);
				off_LED(LED_D2_GREEN);

				if( PINE & _BV(PORTE3) )	// pin is high - no interruption
				{
					off_LED(LED_D5_RED);
					on_LED(LED_D5_GREEN);				
				}else{
					off_LED(LED_D5_GREEN);
					on_LED(LED_D5_RED);				
					input=S1_INTERRUPTED;
			
				}
				break;
			case LOCK_OPENED:
				on_LED(LED_D2_GREEN);
				off_LED(LED_D2_RED);

				if((PINE & (_BV(PORTE1))) )	// pin is high - no interruption
				{
					on_LED(LED_D5_GREEN);
					off_LED(LED_D5_RED);
				}else{
					off_LED(LED_D5_GREEN);
					on_LED(LED_D5_RED);
					input=S2_INTERRUPTED;			
				}
				break;
			case LOCK_DISABLED:
				off_LED(LED_D2_GREEN | LED_D2_RED);
				break
			case LOCK_WAITING:
				on_LED(LED_D2_GREEN | LED_D2_RED);
				input=R_NO_FEEDBACK;
				prev_input=input;
				inputCount=5;
				break;
		}
		
		if (prev_input==input && (input!=0)){
			inputCount++;
		}else{
			inputCount=0;
		}

		if (inputCount==5){
			inputCount=0;
			Event_Signal((EVENT*)input_event);
		}
		Task_Next();
	}

}

void task_controller(){	
	OUTPUTS output=0;
	
	for(;;){
		
		Event_Wait((EVENT*)input_event);

		if(LOCK_NUM=='1'){
			radio_print("Gate %c: state=%d \r\n", LOCK_NUM, state);
			radio_print("Gate %c: input=%d \r\n", LOCK_NUM, input);
		}else{
			radio_print("\t\t\t\tGate %c: input=%d \r\n", LOCK_NUM, input);	
			radio_print("\t\t\t\tGate %c: state=%d \r\n", LOCK_NUM, state);		
		}

		if (input==R_CMD_DISABLE){
			state=LOCK_DISABLED;
		}else{
			switch(state){
				case LOCK_DISABLED:
					if (input==R_CMD_ENABLE){
						output=ENABLE_LOCK;

						radio_print("<%c2>\r\n", prev_lock); 
						delayFor(10);
						radio_print("<%c2>\r\n", prev_lock);
						delayFor(10);
						radio_print("<%c2>\r\n", prev_lock);
					}
					break;
				case LOCK_CLOSED:
					if (input==S1_INTERRUPTED){
						output=OPEN_LOCK;
					}
					break;
				case LOCK_OPENED:
					if (input==S2_INTERRUPTED){
						output=CLOSE_LOCK;
					}
					break;
				case LOCK_WAITING:
					if (input!=R_CMD_FEEDBACK){
						output=SEND_MSG;
					}else{
						state=LOCK_DISABLED;
					}
					break;
			}
		
			if(LOCK_NUM=='1'){
				radio_print("Gate %c: output=%d \r\n", LOCK_NUM, output);
			}else{
				radio_print("\t\t\t\tGate %c: output=%d \r\n", LOCK_NUM, output);			
			}

			switch (output){
				case ENABLE_LOCK:
					state=LOCK_CLOSED;
					break;
				case OPEN_LOCK:
					OCR3B = duty_opened;
					state=LOCK_OPENED;
					break;
				case CLOSE_LOCK:
					OCR3B = duty_closed;
					state=LOCK_WAITING;					
					delayFor(1000); // Waits for completely closed lock
					break;
				default:
					break;
			}

			switch (output){
				case SEND_MSG:
				case CLOSE_LOCK:
					if(LOCK_NUM==LOCK_COUNT){
						radio_print("<%c1>\r\n", next_lock);
					}else{
						radio_print("<%c1>\r\n", next_lock);
					}
					break;
				default:
					break;
			}

		}
		output=0;
		input=0;
		Task_Next();
	}

}

int main(void)
{
	int i=0;
	app_init();

	OCR3B = duty_closed;

	if (LOCK_NUM=='1'){
		state=LOCK_CLOSED;
		
		for (i=0; i<5; i++){
			radio_print("<*0>\r\n");
			delayFor(10);
		}
		
	}

	next_lock=LOCK_NUM+1;
	if (LOCK_NUM==LOCK_COUNT){
		next_lock='1';
	}
	prev_lock=LOCK_NUM-1;
	if (LOCK_NUM=='1'){
		prev_lock='2';
	}

	input_event=Event_Init();

    Task_Create(task_sampler, 1, PERIODIC, A);	
	Task_Create(task_controller, 0, SYSTEM, C);

	Task_Terminate();
}


/**
 * Interrupt service routine for when data arrives at the radio (incoming).
 */
ISR (INT4_vect)
{
    int i;
    int still_good;
    int len;

	uint8_t* buf_ptr;
	uint8_t prevByte, curByte;	


    still_good = 1;
    len = 0;
    buf_ptr = radio_buf;

	
    // Dump the radio to the buffer
    for(i=0; i < PAYLOAD_BYTES; ++i)
    {
		curByte= radio_get_byte();
        *buf_ptr = curByte;
        if(still_good && *buf_ptr == 0)
        {
            len = i;
            still_good = 0;
        }

		prevByte=curByte;
        ++buf_ptr; 
    }
    if(still_good) len = PAYLOAD_BYTES;

    // Redirect to UART
	if (LOCK_NUM=='1'){
    	uart_write(radio_buf, len);
	}

	if (len>3){
		if (radio_buf[0]=='<' && radio_buf[3]=='>'){
			if (radio_buf[1]==LOCK_NUM || radio_buf[1]=='*'){
				radio_cmd=radio_buf[2];
				if (state==LOCK_DISABLED || state== LOCK_WAITING){					
					if (radio_cmd=='1'){
						input=R_CMD_ENABLE;
					}else if (radio_cmd=='2'){
						input=R_CMD_FEEDBACK;
					}else if (radio_cmd=='0'){
						input=R_CMD_DISABLE;
					}				 
					Signal_And_Next((EVENT*)input_event);
				}

			}
		}
	}
}
